import numpy as np

# len_step = 30000
# time_step = np.zeros((len_step, 1))
# time_step[:, 0] = np.linspace(0, 30, len_step)
# x_body = np.zeros((len_step, 1))
# y_body = np.zeros((len_step, 1))
# z_body = np.zeros((len_step, 1))
# roll_body = np.zeros((len_step, 1))
# yaw_body = np.zeros((len_step, 1))
# pitch_body = np.zeros((len_step, 1))
# spin_fr = time_step * 300
# spin_fl = time_step * 300
# spin_rl = time_step * 300
# spin_rr = time_step * 300
# ang_fr = np.zeros((len_step, 1))
# ang_fl = np.zeros((len_step, 1))
# ang_rl = np.zeros((len_step, 1))
# ang_rr = np.zeros((len_step, 1))
# z_fr = np.zeros((len_step, 1))
# z_fl = np.zeros((len_step, 1))
# z_rl = np.zeros((len_step, 1))
# z_rr = np.zeros((len_step, 1))
#
# z_body[:5000, 0] = np.sin(time_step[:5000, 0]) * 0.5
# yaw_body[5000:10000, 0] = np.sin(time_step[5000:10000, 0] * 2) * 10
# pitch_body[15000:20000, 0] = np.sin(time_step[15000:20000, 0] * 2) * 5
# roll_body[10000:15000, 0] = np.sin(time_step[10000:15000, 0] * 2) * 5
# z_fr[5000:10000, 0] = np.sin(time_step[5000:10000, 0]) * 0.3
# z_fl[5000:10000, 0] = np.sin(time_step[5000:10000, 0]) * 0.3
# z_rl[5000:10000, 0] = np.sin(time_step[5000:10000, 0]) * 0.3
# z_rr[5000:10000, 0] = np.sin(time_step[5000:10000, 0]) * 0.3
# ang_fr[15000:20000, 0] = np.sin(time_step[15000:20000, 0]) * 10
# ang_fl[15000:20000, 0] = np.sin(time_step[15000:20000, 0]) * 10
# save_data = np.column_stack((time_step, x_body, y_body, z_body,
#                              roll_body, yaw_body, pitch_body,
#                              spin_fr, spin_fl, spin_rl, spin_rr,
#                              ang_fr, ang_fl, ang_rl, ang_rr,
#                              z_fr, z_fl, z_rl, z_rr))
# np.savetxt('tmp_data.csv', save_data, delimiter=',')

data = np.loadtxt('tmp_data.csv',  delimiter=',')
freq = 30
total_time = data[-1, 0]
len_anime = int(total_time * freq - 1)
anime_time = np.linspace(0, total_time, len_anime)
data_an = np.zeros((len_anime, 18))
i_time = 0
for i in range(1, len_anime):
    while data[i_time, 0] < anime_time[i]:
        i_time += 1
    data_an[i, :] = data[i_time, 1:]
x_body, y_body, z_body, \
    roll_body, yaw_body, pitch_body, \
    spin_fr, spin_fl, spin_rl, spin_rr, \
    ang_fr, ang_fl, ang_rl, ang_rr, \
    z_fr, z_fl, z_rl, z_rr = data_an.T
